#include "parseCMDConfig.hpp"

#include "../Config/configRL.hpp"
#include "report.hpp"
#include "../Misc/misc.hpp"
#include "../Global/globalVars.hpp"
#include "../main/tempCtrlService.hpp"
#include "../main/onlineLoop.hpp"
//#include "../TempControl/tempUpdate.hpp"

bool printConfig(const char* cmdCore);
bool configMetadata(const char* cmdCore);
bool configMiscFlags(const char* cmdCore);
bool configStartUp(const char* cmdCore);
bool configNTCMeta(const char* cmdCore);
bool configNTCCurve(const char* cmdCore);
bool configThermalModel(const char* cmdCore);
bool setControlParas(const char* cmdCore);
bool saveControlParas(const char* cmdCore);
bool setHeartBeatMode(const char* cmdCore);
bool setChangeSpeed(const char* cmdCore);

bool parseCMDConfig(const char* cmdIn)
{
	if (StartWith(cmdIn, "set which")) {
		// make compatible with JM's Identity
		cmdIn += sizeof("set which");
		ConfigRL::MetaDataRL::set(cmdIn, !searchPara(cmdIn, "/q"));
		return true;
	}
	if (!StartWith(cmdIn, "config"))
	{
		return false;
	}
	cmdIn += sizeof("config");
	if (printConfig(cmdIn))
	{
		return true;
	}
	if (configMetadata(cmdIn))
	{
		return true;
	}
	if (configMiscFlags(cmdIn)) {
		return true;
	}
	if (configStartUp(cmdIn)) {
		return true;
	}
	if (configNTCCurve(cmdIn)) {
		return true;
	}
	if (configThermalModel(cmdIn)) {
		return true;
	}
	if (configNTCMeta(cmdIn))
	{
		return true;
	}
	if (setControlParas(cmdIn)) {
		return true;
	}
	if (saveControlParas(cmdIn)) {
		return true;
	}
	if (setHeartBeatMode(cmdIn)) {
		return true;
	}
	if (setChangeSpeed(cmdIn)) {
		return true;
	}
	return false;
}

bool printConfig(const char* cmdCore) {
	if (StartWith(cmdCore, "print"))
	{
		Serial.print(Report::reportStartUpConfig());
		Serial.print(Report::reportHeatUpConfig());
		Serial.print(Report::reportHeatHoldConfig());
		Serial.print(Report::reportCoolDownConfig());
		Serial.print(Report::reportCoolHoldConfig());
		Serial.print(Report::reportThermalModel());
		Serial.print(Report::reportChangeSpeed());
		Serial.flush();
		return true;
	}
	return false;
}

bool configMetadata(const char* cmdCore) {
	if (StartWith(cmdCore, "meta"))
	{
		cmdCore += sizeof("meta");

		//while (strlen(cmdCore) > 0) {
		if (StartWith(cmdCore, "id")) {
			cmdCore += sizeof("id");
			ConfigRL::MetaDataRL::set(cmdCore, !searchPara(cmdCore, "/q"));
			return true;
		}
		//}

		Serial.print(cmdCore);
		Serial.println("pass out? ");
		return true;
	}
	return false;
}

bool configMiscFlags(const char* cmdCore) {
	if (StartWith(cmdCore, "flag"))
	{
		cmdCore += sizeof("flag");
		if (StartWith(cmdCore, "SpOb"))
		{
			cmdCore += sizeof("SpOb");
			if (cmdCore[0] == '?') {
				Serial.print("SpOb is ");
				Serial.println(GlobalVars::flagSampleObMode);
			}
			else {
				int newFlag = atoi(cmdCore);
				GlobalVars::flagSampleObMode = newFlag;
				if (!searchPara(cmdCore, "/q")) {
					Serial.print("SpOb set to ");
					Serial.println(GlobalVars::flagSampleObMode);
				}
			}
			return true;
		}
	}
	return false;
}

// start up to cool to 4
// config startup -1,400,0
bool configStartUp(const char* cmdCore) {
	if (StartWith(cmdCore, "startup"))
	{
		cmdCore += 7 + 1;

		long* values;
		int valueNum;
		myParseInts(cmdCore, &values, &valueNum);

		if (valueNum == 3) {
			ConfigRL::StartUpModeRL::set(values[0], (float)values[1] / 100.0f, values[2], !searchPara(cmdCore, "/q"));
		}
		delete[] values;

		OLLoop::applyStartUpConfig();
		return true;
	}
	return false;
}

bool configNTCMeta(const char* cmdCore) {
	if (StartWith(cmdCore, "NTC "))
	{
		// b,r (measured in kOhm)
		long* values;
		int valueNum;
		myParseInts(cmdCore + 4, &values, &valueNum);
		if (valueNum == 2)
		{
			ConfigRL::SensorCaliRL::writeSurrounding(values[0], values[1] * 1000.0); //???
		}
		delete[] values;
		return true;
	}

	return false;
}

bool configNTCCurve(const char* cmdCore) {
	if (StartWith(cmdCore, "plate. type 0 to quit"))
	{
		Serial.println("#of points:");
		int ptNum = Serial.parseInt();
		if (ptNum <= 0)
		{
			return false;
		}
		float* volts = new float[ptNum];
		float* temps = new float[ptNum];
		for (int idx = 0; idx < ptNum; idx++)
		{
			Serial.print("Volt(mV);temp(K) pair #" + String(idx) + ":");
			float voltVal = Serial.parseFloat();
			float tempVal = Serial.parseFloat();
			Serial.println("{" + String(voltVal) + "};{" + String(tempVal) + "}");
			if (voltVal < 0 || tempVal < 0)
			{
				ptNum = idx;
				Serial.println("Terminated");
			}
			volts[idx] = voltVal;
			temps[idx] = tempVal;
		}
		Serial.print("Now writing " + String(ptNum) + " pairs...");
		ConfigRL::SensorCaliRL::writePlate(temps, volts, ptNum);
		Serial.println("Done");
		delete[] volts;
		delete[] temps;
		return true;
	}

	return false;
}

bool configThermalModel(const char* cmdCore) {
	if (StartWith(cmdCore, "thermal")) {
		cmdCore += sizeof("thermal");

		float newRatio = atof(cmdCore);
		GlobalVars::thermalModel.plateToHolderHeatCapacityRatio = newRatio;
		ConfigRL::ThermalModelMetricsRL::save(!searchPara(cmdCore, "/q"));

		Serial.println(Report::reportThermalModel());

		return true;
	}

	return false;
}

bool setControlParas(const char* cmdCore) {
	if (StartWith(cmdCore, "set curve"))
	{
#ifdef CONTROL_BANGBANG
		cmdCore += sizeof("set curve");
		if (StartWith(cmdCore, "default")) {
			TempCtrlService::init();
			Serial.println("Curve set to default. Not saved.");
			return true;
		}
		Temp2Drive* curve = nullptr;
		if (StartWith(cmdCore, "heat-up")) {
			curve = GlobalVars::heatUpCurve;
		}
		else if (StartWith(cmdCore, "heat-hd")) {
			curve = GlobalVars::heatHoldCurve;
		}
		else if (StartWith(cmdCore, "cool-dn")) {
			curve = GlobalVars::coolDownCurve;
		}
		else if (StartWith(cmdCore, "cool-hd")) {
			curve = GlobalVars::coolHoldCurve;
		}
		if (curve == nullptr) {
			Serial.println("Invalid curve name");
			return false;
		}
		const char* curvePts = cmdCore + 7 + 1;
		long* values;
		int valueNum;
		myParseInts(curvePts, &values, &valueNum);
		if (valueNum > 0)
		{
			if (((BangBangController*)curve)->update(values, valueNum))
			{
				Serial.println("Curve updated. Not saved.");
			}
		}
		else {
			Serial.println("Heat up parse error: not array of ints");
		}
		delete[] values;
#else
		Serial.println("PID control used. No curve to set");
#endif
		return true;
	}
	else if (StartWith(cmdCore, "set pid")) {
		Serial.print("To set pid of ");
		cmdCore = cmdCore + sizeof("set pid");
		TemperatureController* curve = nullptr;
		if (StartWith(cmdCore, "heat-up")) {
			curve = GlobalVars::heatUpCurve;
			Serial.print("heat-up");
		}
		else if (StartWith(cmdCore, "heat-hd")) {
			curve = GlobalVars::heatHoldCurve;
			Serial.print("heat-hold");
		}
		else if (StartWith(cmdCore, "cool-dn")) {
			curve = GlobalVars::coolDownCurve;
			Serial.print("cool-down");
		}
		else if (StartWith(cmdCore, "cool-hd")) {
			curve = GlobalVars::coolHoldCurve;
			Serial.print("cool-hold");
		}
		if (curve == nullptr) {
			Serial.println("Invalid set name");
			return false;
		}
		auto pidParas = (PIDController*)curve;
		cmdCore = cmdCore + sizeof("heat-up");
		char* cmd;
		pidParas->kp = strtof(cmdCore, &cmd);
		cmd++;
		pidParas->ki = strtof(cmd, &cmd);
		cmd++;
		pidParas->kd = strtof(cmd, &cmd);
		Serial.print(", kp = ");
		Serial.print(pidParas->kp);
		Serial.print(", ki = ");
		Serial.print(pidParas->ki);
		Serial.print(", kd = ");
		Serial.print(pidParas->kd);
		Serial.println();
		return true;
	}
	return false;
}

bool saveControlParas(const char* cmdCore) {
	const char* curveCmd0 = "save curve";
	const char* curveCmd1 = "save pid set";
	if (StartWith(cmdCore, curveCmd0) || StartWith(cmdCore, curveCmd1))
	{
		int nameOffset = strlen(curveCmd0) + 1;
		if (StartWith(cmdCore, curveCmd1)) {
			nameOffset = strlen(curveCmd1) + 1;
		}
		const char* curveName = cmdCore + nameOffset;
		if (StartWith(curveName, "heat-up")) {
			ConfigRL::HeadUpCurveRL::save(!searchPara(cmdCore, "/q"));
			return true;
		}
		else if (StartWith(curveName, "heat-hd")) {
			ConfigRL::HeadHoldCurveRL::save(!searchPara(cmdCore, "/q"));
			return true;
		}
		else if (StartWith(curveName, "cool-dn")) {
			ConfigRL::CoolDownCurveRL::save(!searchPara(cmdCore, "/q"));
			return true;
		}
		else if (StartWith(curveName, "cool-hd")) {
			ConfigRL::CoolHoldCurveRL::save(!searchPara(cmdCore, "/q"));
			return true;
		}
		else if (StartWith(curveName, "all")) {
			ConfigRL::HeadUpCurveRL::save(!searchPara(cmdCore, "/q"));
			ConfigRL::HeadHoldCurveRL::save(!searchPara(cmdCore, "/q"));
			ConfigRL::CoolDownCurveRL::save(!searchPara(cmdCore, "/q"));
			ConfigRL::CoolHoldCurveRL::save(!searchPara(cmdCore, "/q"));
			return true;
		}
		Serial.print("Invalid curve name ");
		Serial.println(curveName);
		return false;
	}
	return false;
}

bool setHeartBeatMode(const char* cmdCore) {
	if (StartWith(cmdCore, "heart_beat_interval")) {
		cmdCore = cmdCore + sizeof("heart_beat_interval");
		GlobalVars::MiscFlags::heartBeatInterval = atoi(cmdCore);
		GlobalVars::heartBeatUpdate();
		return true;
	}
	return false;
}

bool setChangeSpeed(const char* cmdCore) {
	if (StartWith(cmdCore, "change speed")) {
		cmdCore += sizeof("change speed");

		float newSpeed = atof(cmdCore);
		GlobalVars::ctrlPlan.ChangeTempSpeed = newSpeed;
		ConfigRL::ChangeTempSpeedRL::save(!searchPara(cmdCore, "/q"));

		Serial.println(Report::reportChangeSpeed());

		return true;
	}
	return false;
}